#include <uav.h>

Uav::Uav()
{
    // malloc space for pointers
    pose = new geometry_msgs::PoseStamped();
    nowpose = new geometry_msgs::PoseStamped();
    cmdposes = new std::vector<uav::Cmd>();
	velocity = new geometry_msgs::Twist();
    scan_ranges = new double[360]();
    sonar_range = new double();

    // initialize the variables
    pose->pose.position.x = 0;
    pose->pose.position.y = 0;
    pose->pose.position.z = 0;
    velocity->linear.x = 0.0;
    velocity->linear.y = 0.0;
    velocity->linear.z = 0.0;

    for (int i=0;i<360;i++){
        cos_scan[i] = cos((i-180)/180.0*M_PI);
        sin_scan[i] = sin((i-180)/180.0*M_PI);
    }

}

// Uav::~Uav(){
//     delete[] pose;
//     delete[] nowpose;
//     delete[] velocity;
//     delete[] cmdposes;
// }
void Uav::Setup(const int& id, const ros::NodeHandle& nh, ros::Rate& rate)
{
    std::string name = "uav" + std::to_string(id);
    uavid = id;
    nh_ = nh;
    rate_ = &rate;

      // define subscribers and publishers
    state_sub = nh_.subscribe<mavros_msgs::State>
            (name + "/mavros/state", 20, &Uav::state_cb, this);
    position_sub = nh_.subscribe<geometry_msgs::PoseStamped>
            (name +"/mavros/local_position/pose", 20, &Uav::pose_cb, this);
    cmd_sub = nh_.subscribe<uav::Cmd>
            ("/uav/cmd", 20, &Uav::cmd_cb, this);
    scan_sub = nh_.subscribe<sensor_msgs::LaserScan>
            (name +"/laser/scan", 20, &Uav::scan_cb, this);
    sonar_sub = nh_.subscribe<sensor_msgs::Range>
            (name +"/sonar", 20, &Uav::sonar_cb, this);
    local_pos_pub = nh_.advertise<geometry_msgs::PoseStamped>
            (name + "/mavros/setpoint_position/local", 20);
    local_vel_pub = nh_.advertise<geometry_msgs::Twist>
            (name + "/mavros/setpoint_velocity/cmd_vel_unstamped", 20);
    arming_client = nh_.serviceClient<mavros_msgs::CommandBool>
            (name + "/mavros/cmd/arming");
    set_mode_client = nh_.serviceClient<mavros_msgs::SetMode>
            (name + "/mavros/set_mode");
}

void Uav::home(double x,double y,double z){
    cmdpose.uavid = uavid;
    cmdpose.x = x;
    cmdpose.y = y;
    cmdpose.z = 1;
    cmdposes->push_back(cmdpose);
    initx = x;
    inity = y;
    initz = z;
}

// callback function to recieve the target position command from external program
void Uav::cmd_cb(const uav::Cmd::ConstPtr& msg){
    if (msg->uavid == uavid){
        // if position control: add message to the waiting-list
        if (msg->type == 0){
            cmdpose.uavid = uavid;
            cmdpose.x = msg->x;
            cmdpose.y = msg->y;
            cmdpose.z = msg->z;
            cmdposes->insert(cmdposes->begin(), cmdpose);
            cmdposes->erase(cmdposes->begin()+1);
        }
        // if velocity control: send msg directly
        else if (msg->type == 1){
            send_vel_cmd(msg->x, msg->y, msg->z);
        }
        // if position control: add message to the waiting-list
        if (msg->type == 2){
            cmdpose.uavid = uavid;
            cmdpose.x = msg->x;
            cmdpose.y = msg->y;
            cmdpose.z = msg->z;
            if (cmdpose != *cmdposes->rbegin()){
                cmdposes->push_back(cmdpose);
            }
        }
    }
}

// callback function to recieve the instance connection signal
void Uav::state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

// wait for the connection signal
void Uav::wait_for_FCU()
{
    while(ros::ok() && !current_state.connected){
        printf("connecting...\n");
        ros::spinOnce();
        rate_->sleep();
    }
}

// send some points to ensure the offboard mode
void Uav::send_setpoints()
{
    for(int i = 20; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(*pose);
        ros::spinOnce();
        rate_->sleep();
    }
}

void Uav::set_mode()
{
    offb_set_mode.request.custom_mode = "OFFBOARD";
}

void Uav::set_arm_cmd()
{
    arm_cmd.request.value = true;
}

void Uav::enable_offboard()
{
    if (current_state.mode != "OFFBOARD"){
        set_mode_client.call(offb_set_mode);
    }
}

void Uav::enable_armed()
{
    if (!current_state.armed){
        arming_client.call(arm_cmd);
    }
}

void Uav::init()
{
    wait_for_FCU();
    send_setpoints();
    set_mode();
    set_arm_cmd();
    enable_armed();
    enable_offboard();
    std::cout<<uavid<<": init success"<<std::endl;
}

void Uav::send_vel_cmd(double vx, double vy, double vz){
    velocity->linear.x = vx;
    velocity->linear.y = vy;
    velocity->linear.z = vz;
    local_vel_pub.publish(*velocity);
}

void Uav::fly_2_target()
{
    // ensure the publishing position is the first of <cmdposes>
    pose->pose.position.x = cmdposes->begin()->x - initx;
    pose->pose.position.y = cmdposes->begin()->y - inity;
    pose->pose.position.z = cmdposes->begin()->z - initz;

    // // echo for monitor
    // std::cout<<"uav: "<<uavid<<"| arrival: "<<arrival<<"| cmd waiting-list:"<<cmdposes->size()<<std::endl;
    // printf("position now : (%.4f,%.4f,%.4f)\n", nowpose->pose.position.x, nowpose->pose.position.y, nowpose->pose.position.z);
    // printf("position next: (%.4f,%.4f,%.4f)\n", pose->pose.position.x, pose->pose.position.y, pose->pose.position.z);

    // publish and update
    local_pos_pub.publish(*pose);
    // ros::spinOnce();
    // rate_.sleep();
}


void Uav::pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    // obtain the present local position
    nowpose->pose.position.x=msg->pose.position.x;
    nowpose->pose.position.y=msg->pose.position.y;
    nowpose->pose.position.z=msg->pose.position.z;

    // compare the present position and target position
    float dx=abs(pose->pose.position.x-nowpose->pose.position.x);
    float dy=abs(pose->pose.position.y-nowpose->pose.position.y);
    float dz=abs(pose->pose.position.z-nowpose->pose.position.z);
 

    if(dx<=0.2&&dy<=0.2&&dz<=0.2)
    {
        check_number++;     // used to ensure the arrival stability
        if(check_number>=max_check_number){
            arrival=true;
            // if there is some commands waiting -> goto next and reset
            if (cmdposes->size()>1){    
                cmdposes->erase(cmdposes->begin());
                arrival=false;
                check_number=0;
            // else : hold the last command until new one comes
            }
        }
    }
}

// calculate the equivalent barrier position from laser data
void Uav::scan_cb(const sensor_msgs::LaserScan::ConstPtr& msg){
    for (int i=0; i<360;i++){
        scan_ranges[i] = msg->ranges[i];
    }
}

void Uav::sonar_cb(const sensor_msgs::Range::ConstPtr& msg){
    *sonar_range = msg->range;
}


// print local position 
void Uav::print_local_pose()
{   
    std::cout<<"uavid:"<<uavid;
    std::cout<<" ;x:"<<nowpose->pose.position.x;
    std::cout<<" ;y:"<<nowpose->pose.position.y;
    std::cout<<" ;z:"<<nowpose->pose.position.z;
    std::cout<<std::endl;
}

// add some new positions to the front of <cmdposes> reversely
void Uav::add_target_front(int cmd_num, double* x, double* y, double* z){

    for (int i=cmd_num-1;i>=0;i--){
        cmdpose.uavid = uavid;
        cmdpose.x = x[i];
        cmdpose.y = y[i];
        cmdpose.z = z[i];
        cmdposes->insert(cmdposes->begin()-1, cmdpose);
    }
}

// add some new positions to the last of <cmdposes> formally
void Uav::add_target_last(int cmd_num, double* x, double* y, double* z){

    for (int i=0;i<cmd_num;i++){
        cmdpose.uavid = uavid;
        cmdpose.x = x[i];
        cmdpose.y = y[i];
        cmdpose.z = z[i];
        cmdposes->push_back(cmdpose);
    }
}

// add APF barrier
void Uav::add_APF_barrier(double x, double y, double z, double coff){
    // v = 0                       ,  dis > 4.0
    //   = -coff* (1/dis^2)* vec(n),  0.5 < dis < 4.0
    //   = -coff*         4* vec(n),  0.0 < dis < 0.5
    // default coff=4.0 : when dis==2.0, abs(v)=1
    double factor;
    double dx = nowpose->pose.position.x - x;
    double dy = nowpose->pose.position.y - y;
    double dz = nowpose->pose.position.z - z;
    double dis = sqrt(dx*dx+dy*dy+dz*dz);
    
    if (dis>4.0) factor = 0.0;
    else factor = dis>0.5?(coff/dis/dis):(coff*4);

    velocity->linear.x += dx/dis*factor;
    velocity->linear.y += dy/dis*factor;
    velocity->linear.z += dz/dis*factor;
}

// add APF target
void Uav::add_APF_target(double x, double y, double z, double coff){
    // v = coff* vec(n),  dis > 1.0
    //     dis * vec(n),  dis < 1.0
    double factor;
    double dx = x - nowpose->pose.position.x;
    double dy = y - nowpose->pose.position.y;
    double dz = z - nowpose->pose.position.z;
    double dis = sqrt(dx*dx+dy*dy+dz*dz);

    factor = dis<1.0?dis:coff;

    velocity->linear.x += dx/dis*factor;
    velocity->linear.y += dy/dis*factor;
    velocity->linear.z += dz/dis*factor;
}

// add APF target from cmdposes
void Uav::add_APF_cmdpose(){
    // ensure the publishing position is the first of <cmdposes>
    pose->pose.position.x = cmdposes->begin()->x - initx;
    pose->pose.position.y = cmdposes->begin()->y - inity;
    pose->pose.position.z = cmdposes->begin()->z - initz;
    add_APF_target(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z);
}

// add APF force from laser data
void Uav::add_APF_scan(){

    double theta = atan2(velocity->linear.y,velocity->linear.x);//-pi to pi
    int v_angle = theta*180/M_PI + 180;  // hrizontal velocity (rad to deg)

    int danger_angle;       // angle of the shortest distance
    int delta_angle = 0;    // offset of danger_angle and vangle
    double range;    
    double min_range = 100.0; // the shortest distance
    for(int i = -90;i <= 90; i++)
    {
        range = scan_ranges[(360 + v_angle + i)%360];
        if(range < min_range){
            delta_angle = i;
            min_range = range;
        }
    }
    danger_angle = (360 + v_angle + delta_angle)%360;
    
    // if uav is dangerous with a barrier
    if(min_range < 2.0 && nowpose->pose.position.z>0.5){
        // make v_n = 0 && keep v_t
        double v_n = (velocity->linear.x*cos_scan[v_angle]+velocity->linear.y*sin_scan[v_angle])*cos_scan[(delta_angle+360)%360];
        velocity->linear.x += v_n * cos_scan[danger_angle];
        velocity->linear.y += v_n * sin_scan[danger_angle];
        // if v_angle is close to danger_angle: fly up
        // otherwise, the uav will stand 
        if(abs(delta_angle)< 15){
            velocity->linear.z = 1.0;
            scan_count = 10;
        }
    }
    // if uav is higher than barrier, still need to fly up for a period
    else if (scan_count>0){
        velocity->linear.z = 1.0;
        scan_count--;
    }

    // if barrier is below: do not fly down
    if (*sonar_range<1 && velocity->linear.z<0 && pose->pose.position.z!=0){
        velocity->linear.z = 0.0;
    }
}

void Uav::send_APF_cmd(){

    local_vel_pub.publish(*velocity);

}